#include <iostream>
#include <GL/glut.h>
#include <unistd.h>
#include <cmath>
#include <vector>
#include "include/Vector.h"
#include "include/Matrix.h"

#define MILISLEEP 2000
#define MEPSLON 1e-5

int niter = 0;

float x_min = -10.0, x_max = 10.0; // Dominio de visualizacao
float y_min = -10.0, y_max = 10.0; // Dominio de visualizacao

Matrix A (2,2);
float  dataA[] = { 2, 1, 1, 3};
float  dataB[] = { 3, 4};
Vector b(2);
float dataChute[] = {8,-8};
Vector v(2);
Vector r(2);
float solucao[] = {1.0,1.0};

using namespace std;

vector<Vector> pontos;
vector<Vector> curva;

void initGL(void)
{
    glClearColor(1.0,1.0,1.0, 1.0);
}

void reshapeCall( int w, int h )
{
  glViewport( 0, 0, w, h ) ;

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  if( w > h )
  {
    double aspect_ratio = (double)w / h ;
    glOrtho(x_min*aspect_ratio, x_max*aspect_ratio, y_min, y_max, -1.0, 1.0);
  }
  else
  {
    double aspect_ratio = (double)h / w ;
    glOrtho(x_min, x_max, y_min*aspect_ratio, y_max*aspect_ratio, -1.0, 1.0);
  }

  glMatrixMode(GL_MODELVIEW);
}

void displayCall()
{
  glClear(GL_COLOR_BUFFER_BIT);

  glLoadIdentity();
  glLineWidth( 1.0 ) ;
  glPointSize(4.0);

    // draw axes
    glBegin( GL_LINES ) ;
    {
      glColor3f(0,1,0);
      glVertex3f( x_min, 0.0, 0.0 ) ;
      glVertex3f( x_max, 0.0, 0.0 ) ;

      glColor3f(0,0,1);
      glVertex3f(  0.0, y_min, 0.0 ) ;
      glVertex3f(  0.0, y_max, 0.0 ) ;
    }
    glEnd() ;

    glColor3f(1.0,0.0,0.0);
    glBegin( GL_POINTS ) ;
    glVertex2f(  solucao[0], solucao[1] ) ;
    glEnd() ;

    glColor3f(0.0,0.0,0.0);
    glBegin( GL_POINTS ) ;
    for(int i = 0; i < pontos.size(); ++i)
    {
        glVertex2f(  pontos[i](0), pontos[i](1) ) ;
    }
    glEnd() ;

    glBegin( GL_LINES ) ;
    for(int i = 0; i < pontos.size()-1; ++i)
    {
        glVertex2f(  pontos[i](0)  , pontos[i](1)   ) ;
        glVertex2f(  pontos[i+1](0), pontos[i+1](1) ) ;

    }
    glEnd() ;

    glPointSize(1.0);
    glBegin( GL_POINTS ) ;
    for(int i = 0; i < curva.size(); ++i)
    {
        glVertex2f(  curva[i](0), curva[i](1) ) ;
    }
    glEnd() ;

  glutSwapBuffers();
}

void addCurva(float k)
{
    int nAmostras = 4000;

    float dy = (y_max - y_min)/(nAmostras);
    float x = x_min, x1 = x_max;

    for(int j = 0; j < nAmostras; ++j)
    {
        float y = j*dy + y_min;

        Vector v1(2);
        v1.transpose();
        Vector v2 = v1;
        v1(0) = x;
        v2(0) = x1;
        v1(1) = v2(1) = y;
        Vector d = v2-v1;
        float e = Vector::dot(A*d,d)/2.0;
        float f = Vector::dot(A*v1,d) - Vector::dot(b,d);
        float g = Vector::dot(A*v1,v1)/2.0 - Vector::dot(b,v1) - k;

        float delta = f*f - 4.0*e*g;
        if(delta < 0.0)
            continue;

        float t1 = (-f - sqrtf(delta) ) / (2*e);
        float t2 = (-f + sqrtf(delta) ) / (2*e);

        Vector p1 = v1 + t1*d;
        Vector p2 = v1 + t2*d;
        curva.push_back(p1);
        curva.push_back(p2);
    }
}

void metodoGradiente()
{
    r = A*v - b;
    float d = Vector::dot(r,r)/Vector::dot(A*r,r);
    v = v + d*(-r);
    pontos.push_back(v);
    addCurva( Vector::dot(A*v,v)/2.0 - Vector::dot(b,v) );
}

void timerCall( int arg )
{
  metodoGradiente();
  if( r.length() < MEPSLON )
  {
      cout << niter+1 << " iteracoes. ";
      cout << v << endl;
      niter = 0;
      v.set(dataChute);
      pontos.clear();
      pontos.push_back(v);
      curva.clear();
  }

  niter++;

  glutTimerFunc(MILISLEEP, timerCall, 0) ;
  glutPostRedisplay() ;
}

using namespace std;

void oglSetup(int argc, char **argv)
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
    glutInitWindowSize(600, 400);
    glutInitWindowPosition(300, 200);
    glutCreateWindow("Aula");
    glutTimerFunc(MILISLEEP, timerCall, 0) ;

    initGL();
    glutDisplayFunc(displayCall) ;
    glutReshapeFunc(reshapeCall) ;
}

int main(int argc, char **argv)
{
    oglSetup(argc, argv);

    A.set(dataA);
    b.set(dataB);
    v.set(dataChute);
    v.transpose();
    b.transpose();

    pontos.push_back(v);

    glutMainLoop();

    return 0;
}
